#include "Orion20m.h"
#include "cvel.h"
void ORION::EulerToQuaternion(double yaw, double pitch, double roll) 
		{ 
			double cy = cos(yaw * 0.5); 
			double cp = cos(pitch * 0.5); 
			double cr = cos(roll * 0.5); 
			double sy = sin(yaw * 0.5); 
			double sp = sin(pitch * 0.5); 
			double sr = sin(roll * 0.5); 
 
			double qw = cy*cp*cr + sy*sp*sr; 
			double qx = sy*cp*cr - cy*sp*sr; 
			double qy = cy*sp*cr + sy*cp*sr; 
			double qz = cy*cp*sr - sy*sp*cr; 
			dprint ("x %f, y %f, z %f, w %w",qx,qy,qz,qw);
			//return new Quaternion((float)qx, (float)qy, (float)qz, (float)qw); 
		} 

	//rotate about x axis
VECTOR3 ORION::rotatex(VECTOR3 dir,double ang){
	return _V(dir.x,cos(ang)*dir.y - sin(ang)*dir.z, sin(ang)*dir.y + cos(ang)*dir.z);
}

VECTOR3 ORION::rotatey(VECTOR3 dir,double ang){
	return _V(cos(ang)*dir.x + sin(ang)*dir.z, dir.y, -sin(ang)*dir.x + cos(ang)*dir.z);
}

VECTOR3 ORION::rotatez(VECTOR3 dir,double ang){
	return _V(cos(ang)*dir.x - sin(ang)*dir.y, sin(ang)*dir.x + cos(ang)*dir.y, dir.z);
}
VECTOR3 ORION::yawpitch2cart(double yaw, double pitch) {
	return _V(
		sin(yaw)*cos(pitch),
		sin(pitch),
		cos(yaw)*cos(pitch)
		);
}

double ORION::dirrot2angle(VECTOR3 v,VECTOR3 h)
{
        // The angle between two vectors can be calculated as either
        // acos ( (v dot h) / (length (v) * length (h)) ) or
        // asin ( length (v cross h) / (length (v) * length (h)) )
        // The dot product version is faster to compute but the acos function
        // gets impresice near an angle of zero.  For this autopilot small
        // angles are the usual case and accuracy is critical.
        VECTOR3 w = crossp(v, h);
        double x = length(w) / (length(h)*length(v));
        if (x>1.0)  return PI; // Avoid precision problems
        else if (x<-1.0) return -PI;
        else return asin(x);
}
